% -------------------------------------------------------------------------
% solve low thrust transfer with 2 body dynamics
%
% [F , J] = fsolve_mee_(p0 , auxdata)
%
% Input argumuents:
% -------------------------------------------------------------------------
% p0             [8x1]            initial co-states             [  -  ]
% auxdata        [structure]      parameters                    [  -  ]
%
% Output argumuents:
% -------------------------------------------------------------------------
% F              [8x1]            shooting constraints          [  -  ]
% J              [8x8]            jacobian matrix               [  -  ]
%
% External functions called:
% -------------------------------------------------------------------------
% ode_yphi_.m
% ode_yphi__mex.mexw32
%
% Copyright(C) 2015/06/22 by Chen Zhang,
% School of Astronautics, Beihang University
% chenzhang.buaa@gmail.com
% -------------------------------------------------------------------------
function [F , J] = fsolve_mee_(p0 , auxdata)
% evaluate constraints
F = Eval_F_(p0 , auxdata);
% calculate gradient of constraints
J = zeros(size(F , 1) , size(p0 , 1));
if nargout > 1
    delta = 1e-4;
    for i = 1 : size(p0 , 1)
        ddelta = zeros(size(F));
        ddelta(i) = delta;
        F_plus = Eval_F_(p0 + ddelta, auxdata);
        F_minus = Eval_F_(p0 - ddelta, auxdata);
        J(:, i) = (F_plus - F_minus) / (2*delta) ;
    end
end
end
function F = Eval_F_(p0 , auxdata)
auxdata.tf = p0(end);
% [tt, yy, ErrorFlag] = ode78_([0 , auxdata.tf] , [auxdata.mee0 ; p0(1 : 7)] , auxdata);
[tt, yy, ErrorFlag] = ode78__mex([0 , auxdata.tf] , [auxdata.mee0 ; p0(1 : 7)] , auxdata);
yy = yy';
yf = yy(: , end);
% calculate switching function
[Sf , Af , ~] = Sw_(yf , auxdata);
if Sf < 0
    u = 1;
else
    u = 0;
end
% calculate hamiltonian
Hamiltonian = 1 + yy(8 : 13 , end)' * Af + u * Sf;
% calculate shooting constraints
F = [yy(1 : 5 , end) - auxdata.meef;
    yy(13 , end);
    yy(14 , end);
    Hamiltonian];
end
% -------------------------------------------------------------------------
